#include "WPILib.h"
#include "Robot2489.h"


void Robot2489::OperatorControl(void) {
	myRobot->SetSafetyEnabled(true);
	m_watchdog.Feed();
	//initial turrent wheel speeds
	potValue = enhancedIO->GetAnalogIn(2);
	bottomWheelValue = (potValue + 1.067) / 4.267;
	
	timer->Start();
	while (IsOperatorControl())
	{
		m_watchdog.Feed();		

		//Activation confirmation
		screen->PrintfLine(DriverStationLCD::kUser_Line1, "Jigglybot: Teleop");
		screen->UpdateLCD();
		
		UpdateSpeeds(); 
		
		myRobot->TankDrive(leftValue, rightValue); //not negative because joysticks negative
		ControlConveyorBelt();                     ///////////////////
		ControlRampTool();                         //Control all the//
		ControlTurretTurn();                       ///////stuff///////
		ControlTurretFiring();                     ///////////////////
		ControlIntake();
		DisplayGearToothVelocity();
		

		//Test file output
		if (gameStick->GetRawButton(9)){
			TestImages();
			fflush(debugFile);
		}

		if (gameStick->GetRawButton(5)){
			ringLight->Set(true);
		}
		else if (gameStick->GetRawButton(6)){
			ringLight->Set(false);
		}
		
		if (gameStick->GetRawButton(1)){

			myRobot->TankDrive(0.0,0.0); //Stop the robot
			ringLight->Set(true);
			Wait(0.1);
			
			InitializeImages();
			DetectRectangles();  //Find backboards
			
		ringLight->Set(false);
			if (reportCount >= 1 && reportCount <= 4 ){
				ProcessLocation();
				//StartAutoAimTurret();           //Start aiming loops, and set turret values
			}

			DeleteImages(); //Clean up memory
		}
		screen->UpdateLCD();
		
		//}
		
		//If image button is pressed, run image detection, and start aiming loops
		//else if (gameStick->GetRawButton(1)){
		//}
	screen->PrintfLine(DriverStationLCD::kUser_Line2,"L:%0.3f R:%0.3f", leftValue, rightValue);
//	screen->PrintfLine(DriverStationLCD::kUser_Line4,"Right: %d, Left: %d", !rightTurretLimit->Get(), !leftTurretLimit->Get());
	screen->PrintfLine(DriverStationLCD::kUser_Line3,"C1: %0.3f, C2: %0.3f", crouch1, crouch2);
//	screen->PrintfLine(DriverStationLCD::kUser_Line5,"Gear Count: %d", gearToothCounter->Get());
	screen->PrintfLine(DriverStationLCD::kUser_Line6, "S: %0.3f", bottomWheelValue);
	screen->UpdateLCD();

	Wait(0.005);				// wait for a motor update time
	}

	fclose(debugFile);
	//timer->Stop();

}
